#include <ros/ros.h>

#include <bullet_rviz_test/stability_checker.h>

int main(int argc, char ** argv)
{
  ros::init(argc, argv, "bullet_rviz_test");
  ros::NodeHandle n;

  bool show_visualization = false;
  bool load_from_file = false;
//  n.param<bool>("/sc_show_visualization", show_visualization, false);
//  n.param<bool>("/sc_load_from_file", load_from_file, false);

//  std::cout << n.getParam("sc_load_from_file", load_from_file) << std::endl;

  bool res;
  bullet_rviz_test::StabilityChecker checker(show_visualization, load_from_file);

  srand(time(NULL));

  while (!checker.has_map()) {
	  std::cout << "no map!" << std::endl;
	  ros::Duration(1.0).sleep();
	  ros::spinOnce();
  }
  for (int i = 0; i < 1000; i++)
    {
      double rand_x = ((double)rand() / RAND_MAX) * 2.0 - 1.0;
      double rand_y = ((double)rand() / RAND_MAX) * 2.0 - 1.0;
      double rand_theta = ((double)rand() / RAND_MAX) * 2.0 - 1.0;

      double x = -3.5 + rand_x;
      double y = 2 + rand_y;
      double theta = M_PI/4.0 + rand_theta;

      res = checker.check(x,y,theta);

      if (res)
      {
        std::cout << "position " << x <<", " << y << ", " << theta << " is ok." << std::endl;
      }
      else
      {
    	  std::cout << "position " << x <<", " << y << ", " << theta << " is NOT ok." << std::endl;
      }
    }

 ros::spinOnce();

  return 0;
}
